using BepuPhysics.Collidables;
using BepuUtilities;
using BepuUtilities.Collections;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics
{
    /// <summary>
    /// 方便的结构,用于直接引用主体的属性。
    /// </summary>
    /// <remarks>Note that this type makes no attempt to protect against unsafe modification of body properties, nor does modifying its properties try to wake up bodies if they are asleep.</remarks>
    public struct BodyReference
    {
        /// <summary>
        /// 此引用引用的正文的句柄。
        /// </summary>
        public BodyHandle Handle;
        /// <summary>
        /// 包含Body的Body集合。
        /// </summary>
        public Bodies Bodies;

        /// <summary>
        /// 构造新的主体引用。
        /// </summary>
        /// <param name="Handle">要引用的正文的句柄。</param>
        /// <param name="Body">包含正文的集合。</param>
        public BodyReference(BodyHandle handle, Bodies bodies)
        {
            Handle = handle;
            Bodies = bodies;
        }

        /// <summary>
        /// 获取主体集中是否存在主体引用。如果句柄映射到同意句柄指向它的有效内存位置,则为true,否则为false。
        /// </summary>
        public bool Exists
        {
            get
            {
                if (Bodies == null)
                    return false;
                return Bodies.BodyExists(Handle);
            }
        }


        /// <summary>
        /// 获取对存储在句柄到位置映射中的正文内存位置的引用。
        /// </summary>
        public ref BodyMemoryLocation MemoryLocation
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                Bodies.ValidateExistingHandle(Handle);
                return ref Bodies.HandleToLocation[Handle.Value];
            }
        }

        /// <summary>
        /// 获取或设置正文是否在活动集中。将其设置为true将尝试唤醒身体;将其设置为false将强制身体和任何连接约束的身体进入睡眠状态。
        /// </summary>
        public bool Awake
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get { return MemoryLocation.SetIndex == 0; }
            set
            {
                if (Awake)
                {
                    if (!value)
                    {
                        Bodies.sleeper.Sleep(MemoryLocation.Index);
                    }
                }
                else
                {
                    if (value)
                    {
                        Bodies.awakener.AwakenBody(Handle);
                    }
                }
            }
        }

        /// <summary>
        /// 获取对身体速度的引用。
        /// </summary>
        public ref BodyVelocity Velocity
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                ref var location = ref MemoryLocation;
                return ref Bodies.Sets[location.SetIndex].Velocities[location.Index];
            }
        }

        /// <summary>
        /// 获取对身体姿势的引用。
        /// </summary>
        public ref RigidPose Pose
        {
            [MethodImpl(MethodImplOptions.NoInlining)]
            get
            {
                ref var location = ref MemoryLocation;
                return ref Bodies.Sets[location.SetIndex].Poses[location.Index];
            }
        }

        /// <summary>
        /// 获取身体可碰撞的引用。
        /// </summary>
        public ref Collidable Collidable
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                ref var location = ref MemoryLocation;
                return ref Bodies.Sets[location.SetIndex].Collidables[location.Index];
            }
        }

        /// <summary>
        /// 获取对身体局部惯性的引用。
        /// </summary>
        public ref BodyInertia LocalInertia
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                ref var location = ref MemoryLocation;
                return ref Bodies.Sets[location.SetIndex].LocalInertias[location.Index];
            }
        }

        /// <summary>
        /// 获取对正文活动状态的引用。
        /// </summary>
        public ref BodyActivity Activity
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                ref var location = ref MemoryLocation;
                return ref Bodies.Sets[location.SetIndex].Activity[location.Index];
            }
        }

        /// <summary>
        /// 获取对正文的连接约束列表的引用。
        /// </summary>
        public ref QuickList<BodyConstraintReference> Constraints
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                ref var location = ref MemoryLocation;
                return ref Bodies.Sets[location.SetIndex].Constraints[location.Index];
            }
        }

        /// <summary>
        /// 获取身体是否为运动学的,这意味着其反惯性和质量都为零。
        /// </summary>
        public bool Kinematic { get { return Bodies.IsKinematic(LocalInertia); } }

        /// <summary>
        /// 获取身体是否具有锁定的惯性,这意味着其反惯性张量为零。
        /// </summary>
        public bool HasLockedInertia { get { return Bodies.HasLockedInertia(LocalInertia.InverseInertiaTensor); } }

        /// <summary>
        /// 如果实体是动力学的,则通过将所有反惯性和质量值设置为零并将其激活来使实体处于运动学状态。
        /// 现在仅包含运动学参照的所有连接到实体的约束都将被删除。
        /// 如果身体是运动学的,则不执行任何操作。
        /// </summary>
        public void BecomeKinematic()
        {
            if (!Kinematic)
            {
                Bodies.SetLocalInertia(Handle, default);
            }
        }

        /// <summary>
        /// 将身体的局部惯性设置为提供的惯性。唤醒身体并正确处理动力学状态和运动学状态之间的任何过渡。
        /// 如果实体从动力学移动到运动学,则现在仅包含运动学参考的任何连接到实体的约束都将被删除。
        /// </summary>
        public void SetLocalInertia(in BodyInertia localInertia)
        {
            Bodies.SetLocalInertia(Handle, localInertia);
        }

        /// <summary>
        /// 根据局部惯性和姿势计算身体的世界空间反惯性张量。
        /// </summary>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ComputeInverseInertia(out Symmetric3x3 inverseInertia)
        {
            ref var location = ref MemoryLocation;
            ref var set = ref Bodies.Sets[MemoryLocation.SetIndex];
            ref var localInertia = ref set.LocalInertias[location.Index];
            ref var pose = ref set.Poses[location.Index];
            PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out inverseInertia);
        }

        /// <summary>
        /// 获取身体的描述。
        /// </summary>
        /// <param name="description">正文的说明。</param>
        public void GetDescription(out BodyDescription description)
        {
            Bodies.GetDescription(Handle, out description);
        }

        /// <summary>
        /// 根据描述设置实体的特性。正确处理动力学和运动学之间以及无形状和有形状之间的任何过渡。
        /// 如果实体变为运动学的,则只包含运动学实体的任何约束都将被删除。唤醒身体并在宽广阶段更新其边界。
        /// </summary>
        /// <param name="description">正文的说明。</param>
        public void ApplyDescription(in BodyDescription description)
        {
            Bodies.ApplyDescription(Handle, description);
        }

        /// <summary>
        /// 更改实体的形状。正确处理无形状和有形状之间的过渡。如果身体不活动,它将被强迫唤醒。在宽阔阶段更新躯干的边界。
        /// </summary>
        /// <param name="newShape">要用于正文的新形状的索引。</param>
        public void SetShape(TypedIndex newShape)
        {
            Bodies.SetShape(Handle, newShape);
        }

        /// <summary>
        /// 获取正文边界框的副本。如果实体没有形状,则边界框在float.MaxValue处有一个最小值,在float.MinValue处有一个最大值。
        /// </summary>
        public unsafe BoundingBox BoundingBox
        {
            get
            {
                BoundingBox box;
                if (GetBoundsReferencesFromBroadPhase(out var min, out var max))
                {
                    box.Min = *min;
                    box.Max = *max;
                }
                else
                {
                    box.Min = new Vector3(float.MaxValue);
                    box.Max = new Vector3(float.MinValue);
                }
                return box;
            }
        }

        /// <summary>
        /// 获取指向宽阶段中主体的边界框最小值和最大值的直接指针。如果正文没有形状,则输出NULL。
        /// </summary>
        /// 指向宽阶段中最小边界框的<param name="min">指针。如果正文没有形状,则为NULL。</param>
        /// 指向宽阶段中的最大边界框的<param name="max">指针。如果正文没有形状,则为NULL。</param>
        /// <return>如果正文具有形状和边界,则为True;否则为False。</return>
        public unsafe bool GetBoundsReferencesFromBroadPhase(out Vector3* min, out Vector3* max)
        {
            ref var location = ref MemoryLocation;
            ref var collidable = ref Bodies.Sets[location.SetIndex].Collidables[location.Index];
            if (collidable.Shape.Exists)
            {
                if (location.SetIndex == 0)
                {
                    Bodies.broadPhase.GetActiveBoundsPointers(collidable.BroadPhaseIndex, out min, out max);
                }
                else
                {
                    Bodies.broadPhase.GetStaticBoundsPointers(collidable.BroadPhaseIndex, out min, out max);
                }
                return true;
            }
            else
            {
                // 没有形状,所以不可能有界限。
                min = null;
                max = null;
                return false;
            }
        }

        /// <summary>
        /// 更新身体当前状态的宽广阶段的边界。不包括速度扩展。如果身体没有形状,什么都不做。
        /// </summary>
        /// <remarks>Can be useful if you made modifications to the body's state that you want reflected in the broad phase before the next timestep.
        /// 例如,如果要在直接移动对象后对宽相位执行光线投射,则必须更新它们的边界,否则宽相位边界将过期,光线可能会丢失。</备注>
        public void UpdateBounds()
        {
            Bodies.UpdateBounds(Handle);
        }

        /// <summary>
        /// 按索引将脉冲应用于身体。不会唤醒身体。
        /// </summary>
        /// <param name="pose">要施加脉冲的正文姿势。</param>
        /// <param name="ocity">要施加冲量的身体速度。</param>
        /// <param name="localInertia">要施加冲量的身体局部惯性。</param>
        /// <param name="Pulse">应用于正文的冲动。</param>
        /// <param name="PullseOffset">距离正文中心的世界空间偏移量,以在其施加脉冲。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset, ref BodyInertia localInertia, ref RigidPose pose, ref BodyVelocity velocity)
        {
            PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out var inverseInertiaTensor);
            ApplyLinearImpulse(impulse, localInertia.InverseMass, ref velocity.Linear);
            ApplyAngularImpulse(Vector3.Cross(impulseOffset, impulse), inverseInertiaTensor, ref velocity.Angular);
        }

        /// <summary>
        /// 按索引将脉冲应用于身体。不会唤醒身体。
        /// </summary>
        /// <param name="set">包含要施加脉冲的正文的正文集。</param>
        /// <param name="index">正文集中正文的索引。</param>
        /// <param name="Pulse">应用于正文的冲动。</param>
        /// <param name="PullseOffset">距离正文中心的世界空间偏移量,以在其施加脉冲。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyImpulse(in BodySet set, int index, in Vector3 impulse, in Vector3 impulseOffset)
        {
            ref var localInertia = ref set.LocalInertias[index];
            ref var pose = ref set.Poses[index];
            ref var velocity = ref set.Velocities[index];
            ApplyImpulse(impulse, impulseOffset, ref localInertia, ref pose, ref velocity);
        }

        /// <summary>
        /// 将角冲量应用于角速度。不会唤醒身体。
        /// </summary>
        /// <param name="angularImPulse">应用于速度的脉冲。</param>
        /// <param name="inverseInertia张量">用来变换冲量的逆惯性张量。</param>
        /// <param name="angularVelocity">要修改的角速度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyAngularImpulse(in Vector3 angularImpulse, in Symmetric3x3 inverseInertiaTensor, ref Vector3 angularVelocity)
        {
            Symmetric3x3.TransformWithoutOverlap(angularImpulse, inverseInertiaTensor, out var angularVelocityChange);
            angularVelocity += angularVelocityChange;
        }

        /// <summary>
        /// 将冲量应用于线速度。不会唤醒身体。
        /// </summary>
        /// <param name="Pulse">应用于速度的脉冲。</param>
        /// <param name="InverseMass">用于转换脉冲的反质量。</param>
        /// <param name="linearVelocity">要修改的线速度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public static void ApplyLinearImpulse(in Vector3 impulse, float inverseMass, ref Vector3 linearVelocity)
        {
            linearVelocity += impulse * inverseMass;
        }


        /// <summary>
        /// 将冲量应用于位于给定世界空间位置的实体。不修改活动状态。
        /// </summary>
        /// <param name="Pulse">应用于正文的冲动。</param>
        /// <param name="PullseOffset">应用脉冲的世界空间偏移量。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ApplyImpulse(in Vector3 impulse, in Vector3 impulseOffset)
        {
            ref var location = ref MemoryLocation;
            ApplyImpulse(Bodies.Sets[location.SetIndex], location.Index, impulse, impulseOffset);
        }

        /// <summary>
        /// 将冲量应用于线速度。不会唤醒身体。
        /// </summary>
        /// <param name="Pulse">应用于速度的脉冲。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ApplyLinearImpulse(in Vector3 impulse)
        {
            ref var location = ref MemoryLocation;
            ref var set = ref Bodies.Sets[location.SetIndex];
            ApplyLinearImpulse(impulse, set.LocalInertias[location.Index].InverseMass, ref set.Velocities[location.Index].Linear);
        }

        /// <summary>
        /// 计算附着到实体的偏移点的速度。
        /// </summary>
        /// <param name="Offset">从正文中心到</param>的偏移
        /// <param name="ocity">点附加到主体时的有效速度。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void GetVelocityForOffset(in Vector3 offset, out Vector3 velocity)
        {
            velocity = Velocity.Linear + Vector3.Cross(Velocity.Angular, offset);
        }

        /// <summary>
        /// 将角冲量应用于角速度。不会唤醒身体。
        /// </summary>
        /// <param name="angularImPulse">应用于速度的脉冲。</param>
        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void ApplyAngularImpulse(in Vector3 angularImpulse)
        {
            ref var location = ref MemoryLocation;
            ref var set = ref Bodies.Sets[location.SetIndex];
            ref var localInertia = ref set.LocalInertias[location.Index];
            ref var pose = ref set.Poses[location.Index];
            PoseIntegration.RotateInverseInertia(localInertia.InverseInertiaTensor, pose.Orientation, out var inverseInertia);
            ApplyAngularImpulse(angularImpulse, inverseInertia, ref set.Velocities[location.Index].Angular);
        }
    }
}
